#include "kmeans_def.hpp"
#include "kmeans_impl.hpp"




//Features as per Andres' WACV paper

inline
opt_feat::opt_feat(const std::string in_path, 
		   const std::string in_actionNames,  
		   const std::string in_feat_path,
		   const int in_col, 
		   const int in_row)
:path(in_path), actionNames(in_actionNames), feat_path(in_feat_path), col(in_col), row(in_row), n_samples_tr(17), n_samples_te(8)
{
  THRESH = 0.000001;
  THRESH_2 = col*row*1/100;

}



inline
void
opt_feat::clustering(){
  
    cout << "# clusters: " << N_cent << endl;

  
  
 uword act = 0 ; ///cambiar
 
    
///cargar lista con nombres de videos con multi_acciones
    std::stringstream tmp_ss2;
    tmp_ss2 << path << "/multi_list.txt";
    
     std::stringstream tmp_ss3;
     tmp_ss3 << path << "/list_labels.txt";
     
     std::stringstream tmp_ss4;
     tmp_ss4 << path << "/person_list.txt";
    //cout << tmp_ss2.str()<< endl;
    //getchar();
    videos.load(tmp_ss2.str()); 
    field<std::string> labels;
    labels.load(tmp_ss3.str()); 
    
    field<std::string> person;
    person.load(tmp_ss4.str());
    
    for (uword vi = 0; vi <videos.n_rows; ++vi ){ 
      
      
      //cout << "Size: " << covs.size() << endl;
      std::stringstream tmp_ss3;
      tmp_ss3 << path << videos(vi);
      cout << tmp_ss3.str()<< endl;

      std::stringstream tmp_lbs;
      tmp_lbs << path << labels(vi);
      cout << tmp_lbs.str()<< endl;

      cout << "Calculating Descriptors per video" << endl;
      feature_video(tmp_ss3.str(), tmp_lbs.str() ); //Calculating feat_video_i and lab_fe_vec

      mat mat_features(15,feat_video_i.size());
      vec vec_labels(feat_video_i.size() );
      
      for (uword i = 0; i < feat_video_i.size(); ++i)
      {
	mat_features.col(i) = feat_video_i.at(i);
	vec_labels(i) = lab_fe_vec.at(i);
	
      }


      cout << "Kmeans ... ";
       std::stringstream save_partitions;
       save_partitions<< "./part_per_" << person(vi) << ".dat";
       cout << save_partitions.str() << endl;
       
        std::stringstream save_counts;
	save_counts<< "./counts_per_" << person(vi) << ".dat";
	cout << save_counts.str() << endl;
	
	std::stringstream save_lab;
	save_lab<< "./lab_per_" << person(vi) << ".dat";
	cout << save_lab.str() << endl;
       
       
       
      N_cent = 64;
      kmeans km(mat_features, N_cent);
      km.run(10);
      km.saving( save_partitions.str(), save_counts.str() );
      vec_labels.save( save_lab.str(), raw_ascii ); 
      
      getchar();
      
   }
}


///Change this. Create a Big field or vector to save feature vectors

inline 
void
opt_feat::feature_video(std::string one_video, std::string labels_onevideo) 
{
    feat_video_i.clear();
    lab_fe_vec.clear();
    
    //std::vector < vec > feat_frame;
    cv::VideoCapture capVideo(one_video);
    //cout << one_video << endl;
    
    rowvec arma_multi_labels;
    arma_multi_labels.load(labels_onevideo); //labels are in a frame basis.
    //cout << "Saving original labels "

  
  //double fps = capVideo.get(CV_CAP_PROP_FPS); //get the frames per seconds of the video
  //cout << "Frame per seconds : " << fps << endl;
  
  //cv::namedWindow("MyVideo",CV_WINDOW_AUTOSIZE); //create a window called "MyVideo"
  
  //double frmcount = capVideo.get(CV_CAP_PROP_FRAME_COUNT);
  //cout << "# of frames is: " << frmcount << endl;
  
  if( !capVideo.isOpened() )
  {
    cout << "Video couldn't be opened" << endl;
    return;
  }
  
      int num_frames = capVideo.get(CV_CAP_PROP_FRAME_COUNT);
      cout << num_frames << endl;
      int t = 0;
      
      
      cv::Mat prevgray, gray, flow, cflow, frame, prevflow;
      cv::Mat ixMat, iyMat, ixxMat, iyyMat;
    
    
   for(;;){
     
    t = capVideo.get(CV_CAP_PROP_POS_FRAMES);
    //cout << t << " ";
    
    bool bSuccess = capVideo.read(frame); // read a new frame from video
      
      if (!bSuccess) //if not success, break loop
      	{
	  cout << "Cannot read the frame from video file" << endl;
	  break;
	}
	
      cv::cvtColor(frame, gray, CV_BGR2GRAY);
      
      if( prevgray.data )
      {
	//cout << "Cuando entra aca?? en t= " << t << endl;
	cv::calcOpticalFlowFarneback(prevgray, 
				     gray, 
			      flow, 
			      0.5, //pyr_scale
			      3,   //levels
			      9,   //winsize
			      1,   //iterations
			      5,   //poly_n
			      1.1, //poly_sigma
			      0);  //flags

	cv::Sobel(gray, ixMat, CV_32F, 1, 0, 1);
	cv::Sobel(gray, iyMat, CV_32F, 0, 1, 1);
	cv::Sobel(gray, ixxMat, CV_32F, 2, 0, 1);
	cv::Sobel(gray, iyyMat, CV_32F, 0, 2, 1);
	
	float  ux = 0, uy = 0, vx = 0,  vy = 0;
	float u, v;
	
	if( prevflow.data )
	{
	  
	  for (uword x = 0 ; x < col ; ++x ){
	    for (uword y = 0 ; y < row ; ++y ) {
	      
	      vec features_one_pixel(15);
	      u = flow.at<cv::Vec2f>(y, x)[0];
	      v = flow.at<cv::Vec2f>(y, x)[1];
	      
	      
	      float ix = ixMat.at<float>(y, x);
	      float iy = iyMat.at<float>(y, x);
	      
	      // grad direction &  grad magnitude
	      float gd = std::atan2(std::abs(iy), std::abs(ix));
	      float gm = std::sqrt(ix * ix + iy * iy);
	      
	      // x second grad
	      float ixx = ixxMat.at<float>(y, x);
	      // y second grad
	      float iyy = iyyMat.at<float>(y, x);
	      //du/dt
	      float ut = u - prevflow.at<cv::Vec2f>(y, x)[0];
	      // dv/dt
	      float vt = v - prevflow.at<cv::Vec2f>(y, x)[1];
	      //// divergence &  vorticity
	      //cout << "divergence &  vorticity" << endl;
	      if (x>0 && y>0 )
	      {
		ux = u - flow.at<cv::Vec2f>(y, x - 1)[0];
		uy = u - flow.at<cv::Vec2f>(y - 1, x)[0];
		vx = v - flow.at<cv::Vec2f>(y, x - 1)[1];
		vy = v - flow.at<cv::Vec2f>(y - 1, x)[1];
	      }
	      //int x_submat = x + rec.x;
	      //int y_submat = y + rec.y;
	      //cout << x_submat << "&" << y_submat << endl;
	      
	      
	      features_one_pixel  << x << y << t << abs(ix) << abs(iy) << abs(ixx) 
	      << abs(iyy) << gm << gd <<  u << v << abs(ut) 
	      << abs(ut) << (ux - vy)  << (vx - uy);
	      //features_one_pixel.t().print("Features Current Pixel: ");
	      //getchar();
	      
	      if (!is_finite( features_one_pixel ) ){
		cout << "It's not FINITE... continue???" << endl;
		getchar(); 
	      }
	      
	      // Plotting Moving pixels
	      //cout << " " << gm;
	      // Empirically set to 40
	      if (gm>40) {
		frame.at<cv::Vec3b>(y,x)[0] = 0;
		frame.at<cv::Vec3b>(y,x)[1] = 0;
		frame.at<cv::Vec3b>(y,x)[2] = 255;
		feat_video_i.push_back(features_one_pixel);
		uword lb = arma_multi_labels(t);
		lab_fe_vec.push_back( lb );
		
		}
	    }
	  }

	}
      }
      
      if(cv::waitKey(30)>=0)
	break;
	std::swap(prevgray, gray);
	std::swap(prevflow, flow);//aca esta el problema.... cuando hay flow????
	
	
	//cv::imshow("color", frame);
	//cv::waitKey(5);
	
    }
  }




